package minimax

import (
	"math"

	"gitee.com/extrame/math/minima"
	"gitee.com/extrame/math/toms748"
	"gitee.com/extrame/math/tools"
	"github.com/sirupsen/logrus"
	"gonum.org/v1/gonum/mat"
)

type Minimax struct {
	cfg           *Config
	controlPoints []float64     // Current control points to be used for the next iteration.
	solution      *mat.VecDense // Solution from the last iteration contains all unknowns including the error term.
	zeros         []float64     // Location of points of zero error from last iteration, plus the two end points.
	unknowns      int           // Total number of unknowns.
	// int m_precision;               // Number of bits precision to which the r.zeros and maxima are found.
	max_change_history [2]float64 // Past history of changes to control points.
	Fn                 Fn
	Err                Fn
	Ex                 Fn
	Started            bool
}

//初始化配置和运行函数
func (r *Minimax) Set(cfg *Config, fn Fn) {
	r.cfg = cfg
	r.Fn = fn
	r.unknowns = cfg.OrderNWithPin() + cfg.OrderD + 1
	var data = make([]float64, r.unknowns)
	data[0] = cfg.Ranges[0][0]
	data[r.unknowns-1] = cfg.Ranges[len(cfg.Ranges)-1][1]
	interval := (data[r.unknowns-1] - data[0]) / (float64(r.unknowns) - 1)
	spot := data[0] + interval
	for i := 1; i < len(data); i++ {
		data[i] = spot
		spot += interval
	}
	r.Err = func(z float64) float64 {
		y := r.Fn(z)
		abs := y - (r.Numerator().Evaluate(z) / r.Denominator().Evaluate(z))
		var err float64
		if r.cfg.RelError {
			if y != 0 {
				err = abs / math.Abs(y)
			} else if 0 == abs {
				// we must be at a root, or it's not recoverable:
				err = 0
			} else {
				// We have a divide by zero!
				// Lets assume that f(x) is zero as a result of
				// internal cancellation error that occurs as a result
				// of shifting a root at point z to the origin so that
				// the approximation can be "pinned" to pass through
				// the origin: in that case it really
				// won't matter what our approximation calculates here
				// as long as it's a small number, return the absolute error:
				err = abs
			}
		} else {
			err = abs
		}
		return err
	}
	r.Ex = func(x float64) float64 {
		return -1 * math.Abs(r.Err(x))
	}
	r.max_change_history[0], r.max_change_history[1] = 1, 1
	r.initChebyshev()
}

func (r *Minimax) initChebyshev() {
	//
	// Fill in the r.zeros:
	//
	terms := r.cfg.OrderD + r.cfg.OrderNWithPin()
	r.zeros = make([]float64, terms+2)
	for i := 0; i < terms; i++ {
		cheb := math.Cos(float64(2*terms-1-2*i) * math.Pi / float64(2*terms))
		cheb += 1
		cheb /= 2
		if r.cfg.Skew != 0 {
			cheb = math.Pow(cheb, float64((200+r.cfg.Skew)/200))
		}

		r.zeros[i+1] = r.cfg.Point(cheb)
	}

	r.zeros[0] = r.cfg.Ranges[0][0]
	r.zeros[r.unknowns] = r.cfg.Ranges[len(r.cfg.Ranges)-1][1]
	// perform a regular interpolation fit:
	A := mat.NewDense(terms, terms, nil)
	B := mat.NewVecDense(terms, nil)
	// fill in the y values:
	for i := 0; i < B.Len(); i++ {
		B.SetVec(i, r.Fn(r.zeros[i+1]))
	}
	// fill in powers of x evaluated at each of the control points:
	offsetN := 0
	if !r.cfg.Pin {
		offsetN = 1
	}
	offsetD := offsetN + r.cfg.OrderN
	for i := 0; i < B.Len(); i++ {
		x0 := r.zeros[i+1]
		x := x0
		if !r.cfg.Pin {
			A.Set(i, 0, 1)
		}
		for j := 0; j < r.cfg.OrderN; j++ {
			A.Set(i, j+offsetN, x)
			x *= x0
		}
		x = x0
		for j := 0; j < r.cfg.OrderD; j++ {
			A.Set(i, j+offsetD, -x*B.AtVec(i))
			x *= x0
		}
	}
	var lu mat.LU
	result := mat.NewVecDense(terms, nil)
	lu.Factorize(A)
	err := lu.SolveVecTo(result, false, B)
	if err != nil {
		logrus.Fatalln(err)
	}
	r.solution = mat.NewVecDense(r.unknowns, nil)
	r.solution.CopyVec(result)
	r.solution.SetVec(r.unknowns-1, 0)
	//
	// Now find all the extrema of the error function:
	//

	m_max_error := float64(0)
	//int max_err_location = 0;
	var maxima = make([]float64, r.unknowns)
	for i := 0; i < r.unknowns; i++ {
		x, y := minima.Minima(r.Ex, r.zeros[i], r.zeros[i+1], 32, int(r.cfg.WorkingPercision))
		maxima[i] = x
		rel_err := math.Abs(y)
		if rel_err > m_max_error {
			m_max_error = math.Abs(y)
			//max_err_location = i;
		}
	}
	r.controlPoints = maxima
}

func (r *Minimax) Iter() float64 {
	A := mat.NewDense(r.unknowns, r.unknowns, nil)
	B := mat.NewVecDense(r.unknowns, nil)
	var dataB = make([]float64, r.unknowns)
	for i := 0; i < B.Len(); i++ {
		if r.cfg.Pin && r.controlPoints[i] == 0 {
			if i > 0 {
				r.controlPoints[i] = r.controlPoints[i-1] / 3
			} else {
				r.controlPoints[i] = r.controlPoints[i+1] / 3
			}
			dataB[i] = r.Fn(r.controlPoints[i])
		}
	}

	err_err := float64(0)
	for convergenceCount := 0; r.cfg.OrderD > 0 && convergenceCount < 80 && err_err > 0.001; convergenceCount++ {
		sign := float64(1)
		offsetN := 0
		if !r.cfg.Pin {
			offsetN = 1
		}
		offsetD := offsetN + r.cfg.OrderN
		elast := r.solution.AtVec(r.unknowns - 1)
		maxorder := r.cfg.MaxOrder()
		for i := 0; i < B.Len(); i++ {
			x0 := r.controlPoints[i]
			x := x0
			if !r.cfg.Pin {
				A.Set(i, 0, 1)
			}
			for j := 0; j < maxorder; j++ {
				if j < r.cfg.OrderN {
					A.Set(i, j+offsetN, x)
				}
				if j < r.cfg.OrderD {
					var mult float64
					if r.cfg.RelError {
						mult = B.AtVec(i) - sign*math.Abs(B.AtVec(i))*elast
					} else {
						mult = B.AtVec(i) - sign*elast
					}
					A.Set(i, j+offsetD, -x*mult)
				}
				x *= x0
			}
			var E float64
			if r.cfg.RelError {
				E = sign * math.Abs(B.AtVec(i))
			} else {
				E = sign
			}
			A.Set(i, r.unknowns-1, E)
			sign = -sign
		}
		var lu mat.LU
		lu.Factorize(A)
		lu.SolveVecTo(r.solution, false, B)
		if elast != 0 {
			err_err = math.Abs(math.Abs((r.solution.AtVec(r.unknowns-1))-math.Abs(elast)) / math.Abs(elast))
		} else {
			err_err = 1
		}
	}
	var sanity = mat.NewVecDense(r.unknowns, nil)
	sanity.MulVec(A, r.solution)
	for i := 0; i < B.Len(); i++ {
		err := math.Abs(B.AtVec(i) - sanity.AtVec(i)/math.Abs(B.AtVec(i)))
		if err > math.Sqrt(tools.Epsilon()) {
			logrus.Fatalln("Sanity check failed: more than half the digits in the found solution are in error.")
		}
	}

	num := r.Numerator()
	denom := r.Denominator()
	e1 := B.AtVec(0) - num.Evaluate(r.controlPoints[0])/denom.Evaluate(r.controlPoints[0])
	for i := 1; i < B.Len(); i++ {
		e2 := B.AtVec(i) - num.Evaluate(r.controlPoints[i])/denom.Evaluate(r.controlPoints[i])
		if e1*e2 > 0 {
			perturbation := 0.05
			for perturbation < 0.8 {
				point := r.controlPoints[i]*(1-perturbation) + r.controlPoints[i-1]*perturbation
				e2 = r.Fn(point) - num.Evaluate(point)/denom.Evaluate(point)
				if e1*e2 < 0 {
					r.controlPoints[i] = point
					break
				}
				perturbation += 0.05
			}
			if e2*e1 > 0 && (i+1 < B.Len()) {
				perturbation = 0.05
				for perturbation < 0.8 {
					point := r.controlPoints[i]*(1-perturbation) + r.controlPoints[i+1]*perturbation
					e2 = r.Fn(point) - num.Evaluate(point)/denom.Evaluate(point)
					if e1*e2 < 0 {
						r.controlPoints[i] = point
						break
					}
					perturbation += 0.05
				}
			}
		}
		e1 = e2
	}
	r.zeros[0] = r.cfg.Ranges[0][0]
	r.zeros[r.unknowns] = r.cfg.Ranges[len(r.cfg.Ranges)-1][1]
	for i := 1; i < len(r.controlPoints); i++ {
		var tol = toms748.EpsTolerance(math.Pow10(-int(r.cfg.WorkingPercision)))
		maxIter := 1000
		a, b, _ := toms748.Toms748(r.Err, r.controlPoints[i-1], r.controlPoints[i], tol, maxIter)
		r.zeros[i] = (a + b) / 2
	}
	m_max_error := float64(0)
	//int max_err_location = 0;
	var maxima = make([]float64, r.unknowns)
	for i := 0; i < r.unknowns; i++ {
		x, y := minima.Minima(r.Ex, r.zeros[i], r.zeros[i+1], 32, int(r.cfg.WorkingPercision))
		maxima[i] = x
		rel_err := math.Abs(y)
		if rel_err > m_max_error {
			m_max_error = math.Abs(y)
			//max_err_location = i;
		}
	}
	r.controlPoints, maxima = maxima, r.controlPoints
	m_max_change := float64(0)
	for i := 0; i < r.unknowns; i++ {
		r.controlPoints[i] = (r.controlPoints[i]*float64(100-r.cfg.Brake) + maxima[i]*float64(r.cfg.Brake)) / 100
		change := math.Abs(r.controlPoints[i] - maxima[i])
		if change > m_max_change {
			m_max_change = change
		}
	}
	r.max_change_history[0] = r.max_change_history[1]
	r.max_change_history[1] = math.Abs(m_max_change)
	return m_max_change
}

func (r *Minimax) Numerator() tools.Poly {
	a := make([]float64, r.cfg.OrderN+1)
	if r.cfg.Pin {
		a[0] = 0
	}
	terms := r.cfg.OrderNWithPin()
	for i := 0; i < terms; i++ {
		a[r.cfg.Iter(i)] = r.solution.AtVec(i)
	}
	return tools.Polynomial(a)
}

func (r *Minimax) Denominator() tools.Poly {
	terms := r.cfg.OrderD + 1
	offsetD := r.cfg.OrderNWithPin()
	a := make([]float64, terms)
	a[0] = 1
	for i := 0; i < r.cfg.OrderD; i++ {
		a[i+1] = r.solution.AtVec(i + offsetD)
	}
	return tools.Polynomial(a)
}
